/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package robotics.voronoi;

/**
 *
 * @author fallen
 */
import java.awt.Color;
import java.awt.Point;
import java.awt.image.BufferedImage;
import java.io.File;
import java.io.IOException;
import java.util.Arrays;
import java.util.HashMap;
import java.util.Vector;
import javax.imageio.ImageIO;
import robotics.sandbox.ImageOperations;

public class Voronoi {

    private int[][] a;
    private int n; // size by y
    private int m; // size by x
    private final Direction startingDir;
    private final Point startingPoint;
    private static final IVoronoiMethod vMethod = new Voronoi45();

    //<editor-fold defaultstate="collapsed" desc="min dist finding">
    /**
     * entry point method
     */
    private int getMinDist(Direction curDir, Point pointToCheck) {
        return getLeftRightDistMin(curDir, pointToCheck);//getAllDistMin(curDir, pointToCheck);
    }

    private int getAllDistMin(Direction curDir, Point pointToCheck) {
        System.out.println("getAllForwardDistMin(curDir, pointToCheck) = " + getAllForwardDistMin(curDir, pointToCheck));
        System.out.println("getAllForwardDistMin(curDir.getOpposite(), pointToCheck) = " + getAllForwardDistMin(curDir.getOpposite(), pointToCheck));
        System.out.println("getLeftRightDistMin(curDir, pointToCheck) = " + getLeftRightDistMin(curDir, pointToCheck));
        return getMin(new int[]{
                    getAllForwardDistMin(curDir, pointToCheck),
                    getAllForwardDistMin(curDir.getOpposite(), pointToCheck),
                    getLeftRightDistMin(curDir, pointToCheck)
                });
    }

    private int getLeftRightDistMin(Direction curDir, Point pointToCheck) {
        int lDist = getDistance(curDir.getLeft90(), pointToCheck);
        int rDist = getDistance(curDir.getRight90(), pointToCheck);
        return Math.min(lDist, rDist);
    }

    private int getAllForwardDistMin(Direction curDir, Point pointToCheck) {
        int forwardDist = getDistance(curDir, pointToCheck);
        int luDist = getDistance(curDir.getLeft45(), pointToCheck);
        int ldDist = getDistance(curDir.getLeft135(), pointToCheck);
        int ruDist = getDistance(curDir.getRight45(), pointToCheck);
        int rdDist = getDistance(curDir.getRight135(), pointToCheck);
        return getMin(new int[]{
                    forwardDist, luDist, ldDist, ruDist, rdDist
                });
    }

    private int getMin(int[] arr) {
        Arrays.sort(arr);
        return arr[0];
    }
    //</editor-fold>

    //<editor-fold defaultstate="collapsed" desc="voronoi methods">
    private static interface IVoronoiMethod {

        Direction[] getMoves(Direction dir);
    }

    private static class Voronoi45 implements IVoronoiMethod {

        @Override
        public Direction[] getMoves(Direction dir) {
            return new Direction[]{dir.getLeft45(), dir, dir.getRight45()};
        }
    }

    private static class Voronoi90 implements IVoronoiMethod {

        @Override
        public Direction[] getMoves(Direction dir) {
            return new Direction[]{
                        dir.getLeft90(), dir.getLeft45(), dir, dir.getRight45(), dir.getRight90()};
        }
    }

    private static class Voronoi135 implements IVoronoiMethod {

        @Override
        public Direction[] getMoves(Direction dir) {
            return new Direction[]{
                        dir.getLeft135(), dir.getLeft90(), dir.getLeft45(), dir,
                        dir.getRight45(), dir.getRight90(), dir.getRight135()};
        }
    }

    private static class Voronoi180 implements IVoronoiMethod {

        @Override
        public Direction[] getMoves(Direction dir) {
            return new Direction[]{
                        dir.getLeft135(), dir.getLeft90(), dir.getLeft45(), dir,
                        dir.getRight45(), dir.getRight90(), dir.getRight135(), dir.getOpposite()};
        }
    }
    //</editor-fold>

    private interface EachPointBehaviour {

        public void onEachPoint(Point p);
    }

    private static enum Direction {

        L, R, D, U, LU, LD, RU, RD;
        private int xOffset;
        private int yOffset;
        private Direction l45;

        private void init(int xOffset, int yOffset, String left45) {
            this.xOffset = xOffset;
            this.yOffset = yOffset;
            this.l45 = valueOf(left45);
        }

        static {
            L.init(-1, 0, "LD");
            R.init(1, 0, "RU");
            D.init(0, -1, "RD");
            U.init(0, 1, "LU");
            LD.init(-1, -1, "D");
            LU.init(-1, 1, "L");
            RD.init(1, -1, "R");
            RU.init(1, 1, "U");
        }

        public Direction getLeft45() {
            return l45;
        }

        public Direction getLeft90() {
            return getLeft45().getLeft45();
        }

        private Direction getLeft135() {
            return getLeft90().getLeft45();
        }

        public Direction getOpposite() {
            return getLeft90().getLeft90();
        }

        private Direction getRight135() {
            return getOpposite().getLeft45();
        }

        public Direction getRight90() {
            return getLeft90().getOpposite();
        }

        public Direction getRight45() {
            return getRight90().getLeft45();
        }

        public Point movePoint(Point p) {
            return new Point(p.x + xOffset, p.y + yOffset);
        }
    }

    /**
     * a is an array which contains 0 for empty space and 1 for an obstacles
     */
    public Voronoi(int[][] a, Point startingPoint, Direction startingDir) {
        this.a = a;
        this.startingPoint = startingPoint;
        this.startingDir = startingDir;
        this.n = a.length;
        this.m = a[0].length;
    }

    /**
     * a is an array which contains 0 for empty space and 1 for an obstacles
     */
    public Voronoi(int[][] a) {
        this(a, new Point(a.length / 2, 0), Direction.U);
    }

    public Direction[] getPath() {
        return getPath(Integer.MAX_VALUE);
    }

    public Direction[] getPath(int maxPath) {
        Point curPoint = startingPoint;
        Direction curDir = startingDir;
        Vector<Direction> v = new Vector<Direction>();

        for (int nIter = 0; nIter < maxPath; nIter++) {
            // logging
            println("curPoint = " + curPoint);
            println("a[curPoint.y][curPoint.x] = " + a[curPoint.y][curPoint.x]);
            println("curDir = " + curDir);

            Direction[] moves = filterAvailableMoves(vMethod.getMoves(curDir), curPoint);
            int lDist = getDistance(curDir.getLeft90(), curPoint);
            int rDist = getDistance(curDir.getRight90(), curPoint);
            System.out.println("lDist = " + lDist);
            System.out.println("rDist = " + rDist);

            System.out.println("(Math.abs(lDist - rDist)/Math.max(lDist, rDist)) = " + (Math.abs(lDist - rDist)/Math.max(lDist, rDist)));
            System.out.println("Math.abs(lDist - rDist) = " + Math.abs(lDist - rDist));
            System.out.println("Math.max(lDist, rDist) = " + Math.max(lDist, rDist));
            float absV = Math.abs(lDist - rDist);
            float maxV = Math.max(lDist, rDist);
            if ((absV/maxV)<0.1) {
                if (moveIsAvailable(curDir, curPoint)) {
                    curPoint = movePoint(curDir, curPoint);
                    curDir = curDir;
                    v.add(curDir);

                    println("STRAIGHT\n");
                    continue;
                }
            } else if (lDist > rDist) {
                Direction dL = curDir.getLeft45();
                if (moveIsAvailable(dL, curPoint)) {
                    curPoint = movePoint(dL, curPoint);
                    curDir = dL;
                    v.add(curDir);

                    println("TURN LEFT!\n");
                    continue;
                }
            } else {
                Direction dR = curDir.getRight45();
                if (moveIsAvailable(dR, curPoint)) {
                    curPoint = movePoint(dR, curPoint);
                    curDir = dR;
                    v.add(curDir);

                    println("TURN RIGHT!\n");
                    continue;
                }
            }

            // no available move
            println("nIter = " + nIter);
            break;
        }
        Direction[] r = new Direction[v.size()];
        return v.toArray(r);
    }

    public Direction[] getPathMyInterp(int maxPath) {
        Point curPoint = startingPoint;
        Direction curDir = startingDir;
        Vector<Direction> v = new Vector<Direction>();

        for (int nIter = 0; nIter < maxPath; nIter++) {
            Direction[] moves = filterAvailableMoves(vMethod.getMoves(curDir), curPoint);

            // logging
            println("curPoint = " + curPoint);
            println("a[curPoint.y][curPoint.x] = " + a[curPoint.y][curPoint.x]);
            println("moves unfiltered = " + Arrays.toString(vMethod.getMoves(curDir)));
            println("moves filtered = " + Arrays.toString(moves));

            // no more moves, exit
            if (moves == null || moves.length == 0) {
                println("nIter = " + nIter);
                break;
            }

            // otherwise, find the best move
            int bestIdx = 0;
            int maxMinDist = 0; // maximum from minDistance from all the moves
            for (int i = 0; i < moves.length; i++) {
                int minDist = getMinDist(curDir, movePoint(moves[i], curPoint));

                // logging
                println("moves[i] = " + moves[i]);
                println("minDist = " + minDist);

                if (maxMinDist < minDist) {
                    maxMinDist = minDist;
                    bestIdx = i;
                }
            }

            // logging
            println("moves[bestIdx] = " + moves[bestIdx]);
            if (moves[bestIdx] != curDir) {
                println("TURN!");
            }
            println("");

            // next iter
            curPoint = movePoint(moves[bestIdx], curPoint);
            curDir = moves[bestIdx];
            v.add(curDir);
        }
        Direction[] r = new Direction[v.size()];
        return v.toArray(r);
    }

    private void throughPath(Direction[] dir, EachPointBehaviour behaviour) {
        Direction sd = startingDir;
        Point sp = startingPoint;
        for (int i = 0; i < dir.length; i++) {
            behaviour.onEachPoint(sp);
            sp = dir[i].movePoint(sp);
        }
    }

    public BufferedImage drawPath(Direction[] path, final BufferedImage image, final Color c) {
        throughPath(path, new EachPointBehaviour() {

            @Override
            public void onEachPoint(Point p) {
                image.setRGB(p.x, p.y, c.getRGB());
            }
        });
        return image;
    }

    public static void main(String[] args) throws IOException {
        String path = "trackPhotos\\napohui.bmp";
        BufferedImage img = ImageIO.read(new File(path));

        Voronoi v = new Voronoi(imageToArray(img),
                new Point(img.getWidth() / 2, img.getHeight() - 2), Direction.D); // D in a real image is U

//        Voronoi v = new Voronoi(imageToArray(img),
//                new Point(img.getWidth() - 1, (int)(img.getHeight() / 1.5)), Direction.U);
//        System.out.println(Arrays.toString(v.getPath(100)));

        ImageOperations.drawFrame(v.drawPath(v.getPath(200), img, Color.red));
    }

    //<editor-fold defaultstate="collapsed" desc="utils">
    /**
     * p must be a valid point inside
     * <code>a</code>
     */
    private boolean isObstacle(Point p) {
        return a[p.y][p.x] != 0;
    }

    /**
     * no side effect, returns new point
     */
    private Point movePoint(Direction direction, Point p) {
        int x = p.x;
        int y = p.y;
        return new Point(x + direction.xOffset, y + direction.yOffset);
    }

    /**
     * no side effects, returns a new distance
     */
    private int getDistance(Direction dir, Point p) {
        int i = 0;
        for (; pointIsValid(p) && !isObstacle(p); i++) {
            p = movePoint(dir, p);
        }
        return i;
    }

    /**
     * no side effects
     */
    private Direction[] filterAvailableMoves(Direction[] moves, Point curPoint) {
        Vector<Direction> v = new Vector<Direction>();
        for (int i = 0; i < moves.length; i++) {
            if (moveIsAvailable(moves[i], curPoint)) {
                v.add(moves[i]);
            }
        }
        Direction[] r = new Direction[v.size()];
        return v.toArray(r);
    }

    private boolean moveIsAvailable(Direction move, Point p) {
        Point p1 = movePoint(move, p);
        return pointIsValid(p1) && !isObstacle(p1);
    }

    /**
     * no side effects
     */
    private boolean pointIsValid(Point p) {
        boolean xValid = 0 <= p.x && p.x < m;
        boolean yValid = 0 <= p.y && p.y < n;
        return xValid && yValid;
    }

    private static int[][] revArr(int[][] a) {
        int[][] res = new int[a.length][];
        for (int i = 0; i < a.length; i++) {
            res[i] = a[a.length - i - 1].clone();
        }
        return res;
    }

    private static void println(Object obj) {
        System.out.println(obj);
    }

    private static int[][] imageToArray(BufferedImage image) {
        int n = image.getHeight();
        int m = image.getWidth();
        int[][] arr = new int[n][m];
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < m; j++) {
                int rgb = image.getRGB(j, i);
                arr[i][j] = (rgb == -1) ? 0 : 1;
            }
        }
        return arr;
    }
    //</editor-fold>
    //<editor-fold defaultstate="collapsed" desc="tests">
    //<editor-fold defaultstate="collapsed" desc="test1">
    private static int testArr1[][] = new int[][]{
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}
    };
    //</editor-fold>
    //<editor-fold defaultstate="collapsed" desc="test2">
    private static int testArr2[][] = new int[][]{
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1},
        {0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
        {0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}
    };
    //</editor-fold>
    //<editor-fold defaultstate="collapsed" desc="test3">
    private static int testArr3[][] = new int[][]{
        {0, 0, 0, 1, 1, 1, 1},
        {0, 0, 1, 1, 0, 0, 0},
        {0, 0, 1, 0, 0, 0, 0},
        {0, 0, 1, 0, 0, 1, 1},
        {0, 0, 1, 0, 1, 1, 0},
        {0, 0, 1, 0, 1, 0, 0}
    };
    //</editor-fold>
    //<editor-fold defaultstate="collapsed" desc="test4 180 turn">
    private static int testArr4[][] = new int[][]{
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 1, 1, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 0, 1, 0, 0, 0, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0},
        {0, 1, 0, 0, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0},
        {1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0},
        {1, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0},
        {1, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0, 0},
        {1, 0, 0, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 0},
        {1, 0, 0, 0, 1, 0, 0, 0, -0, 0, 0, 0, 0, 0, 1, 1, 0, 0}
    };
    //</editor-fold>
    //</editor-fold>
}
